import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from px4_msgs.msg import VehicleOdometry
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from geometry_msgs.msg import PoseStamped
class DronePath(Node):
    def __init__(self):
        super().__init__('odom_node')
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )
        self.pub = self.create_publisher(Path,'Path_info',10)
        self.timer = self.create_timer(0.1,self.path_timer_callback)
        self.sub = self.create_subscription(VehicleOdometry,'fmu/out/vehicle_odometry',self.path_callback,qos_profile)

        self.Path = Path()
        self.Path.header.stamp = self.get_clock().now().to_msg()
        self.Path.header.frame_id = 'base_link'

    def path_timer_callback(self):
        try:
            self.pub.publish(self.path)
        except:
            pass

    def path_callback(self,msgs):
        try:
            self.PathPose = PoseStamped()
            self.PathPose.header.stamp = self.get_clock().now().to_msg()
            self.PathPose.header.frame_id = ('drone_1')

            self.PathPose.pose.position.x =  float(msgs.position[0]) 
            self.PathPose.pose.position.y = -float(msgs.position[1])           
            self.PathPose.pose.position.z = -float(msgs.position[2])

            
            # 坐标变换：需要绕x轴旋转90度
            # (0.707+0.707i)(a+bi+cj+dk)(0.707-0.707i) = (a+bi-cj+dk)
            self.PathPose.pose.orientation.w =  float(msgs.q[0])
            self.PathPose.pose.orientation.x =  float(msgs.q[1])
            self.PathPose.pose.orientation.y = -float(msgs.q[3])
            self.PathPose.pose.orientation.z =  float(msgs.q[2])
            self.Path.poses.append(self.PathPose)
            self.pub.publish(self.Path)
        except:
            pass

def main(args = None):
    rclpy.init(args=args)
    node =  DronePath()
    rclpy.spin(node)
    rclpy.shutdown()